#include "dofbot.h"

static ret_t run_test(const char* serial_url, const char* action, const char* param) {
  int32_t i = 0;
  int32_t value = 0;
  ret_t ret = RET_OK;
  dofbot_t* dofbot = dofbot_create(serial_url);
  return_value_if_fail(dofbot != NULL, RET_BAD_PARAMS);
  if (tk_str_eq(action, "reset")) {
    dofbot_reset(dofbot);
  } else if (tk_str_eq(action, "version")) {
    dofbot_read_version(dofbot, &value);
    log_debug("version=%d\n", value);
  } else if (tk_str_eq(action, "read")) {
    for (i = 1; i < 7; i++) {
      dofbot_read_servo(dofbot, i, &value);
      log_debug("%d: value=%d\n", i, value);
    }
  } else if (tk_str_eq(action, "write")) {
    int id = 1;
    int pos = 0;
    int time = 500;
    if (param != NULL) {
      sscanf(param, "%d:%d:%d", &id, &pos, &time);
    }
    dofbot_read_servo(dofbot, id, &value);
    ret = dofbot_write_servo(dofbot, id, pos, time);
    log_debug("%d %d %d ret=%d\n", id, pos, time, ret);
  } else if (tk_str_eq(action, "home")) {
    dofbot_home(dofbot);
  } else if (tk_str_eq(action, "home_servo")) {
    dofbot_home_servo(dofbot, tk_atoi(param));
  } else if (tk_str_eq(action, "buzzer_on")) {
    dofbot_buzzer(dofbot, 10);
  } else if (tk_str_eq(action, "buzzer_off")) {
    dofbot_buzzer(dofbot, 0);
  } else if (tk_str_eq(action, "torque_on")) {
    dofbot_set_torque_mode(dofbot, TRUE);
  } else if (tk_str_eq(action, "torque_off")) {
    dofbot_set_torque_mode(dofbot, FALSE);
  } else if (tk_str_eq(action, "color")) {
    if (param == NULL || *param == 'r') {
      dofbot_set_rgb(dofbot, 50, 0, 0);
    } else if (*param == 'g') {
      dofbot_set_rgb(dofbot, 0, 50, 0);
    } else {
      dofbot_set_rgb(dofbot, 0, 0, 50);
    }
  }

  dofbot_destroy(dofbot);

  return RET_OK;
}

/*
 strace -e read=3,5 -e write=3,5 ./bin/dofbot /dev/myserial  read |grep \|
*/

int main(int argc, char* argv[]) {
  if (argc < 3) {
    printf("Usage: %s url action param\n", argv[0]);
    printf("ex: %s /dev/myserial reset\n", argv[0]);
    printf("ex: %s /dev/myserial buzzer_on\n", argv[0]);
    printf("ex: %s /dev/myserial buzzer_off\n", argv[0]);
    printf("ex: %s /dev/myserial torque_on\n", argv[0]);
    printf("ex: %s /dev/myserial torque_off\n", argv[0]);
    printf("ex: %s /dev/myserial home\n", argv[0]);
    printf("ex: %s /dev/myserial home_servo id\n", argv[0]);
    printf("ex: %s /dev/myserial read\n", argv[0]);
    printf("ex: %s /dev/myserial write 1:60:500\n", argv[0]);
    printf("ex: %s /dev/myserial color red\n", argv[0]);
    return 0;
  }

  tk_socket_init();
  platform_prepare();

  run_test(argv[1], argv[2], argv[3]);

  tk_socket_deinit();

  return 0;
}
